<HTML> 
<HEAD> 
	<TITLE>3rd Party ROBOTC Drivers</TITLE> 
<link href="tabs.css" rel="stylesheet" type="text/css"/> 
<link href="doxygen.css" rel="stylesheet" type="text/css"/> 
</HEAD> 
 
<BODY bgcolor="#ffffff" link="#000000" vlink="#000000"> 
<table width="100%" bgcolor="navy" cellspacing=0 cellpadding=1 border=0> 
<tr><td><table width="100%" bgcolor="#EEEEDD" cellspacing=0 cellpadding=3 border=0> 
<tr> 
    <td width="33%" align="left"> <img src="images/LOGO_NXT.gif" width=266 height=44 border="0" alt="Mindstorms"></td> 
    <td><b><font size="+3" color="navy">3rd Party ROBOTC Drivers</font></b></td> 
    <td align="right"> <img src="images/logo.png" width=44 height=44 border="0" alt="RobotC"></td>     
</tr> 
  <tr bgcolor="#cccc99" align="right"> 
          <td colspan=3> <font face="arial" size="-1"> [<a href=main.html>Home</a>] [<a target=_top href="https://sourceforge.net/projects/rdpartyrobotcdr/">Download</a>] [<a target=_top href="http://apps.sourceforge.net/mantisbt/rdpartyrobotcdr/my_view_page.php">Submit a bug/suggestion</a>]  [<a target=_top href="http://www.robotc.net/forums/">ROBOTC Forums</a>] [<a target=_top href="http://botbench.com">Blog</a>] [<a target=_top href="http://sourceforge.net/donate/index.php?group_id=257238">Support this project</a>]</font> </td> 
</tr> 
</table></td></tr> 
</table> 
<!-- Generated by Doxygen 1.7.2 -->
<div class="header">
  <div class="headertitle">
<h1>benedettelli-nxt2wifi-test4.c</h1>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><span class="preprocessor">#pragma config(Sensor, S1,     TOUCH,          sensorTouch)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Sensor, S2,     COLOUR,         sensorCOLORFULL)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Sensor, S3,     SONAR,          sensorSONAR)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Sensor, S4,     NXT2WIFI,       sensorHighSpeed)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorA,          MOT_ACTION,          tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorB,          MOT_LEFT,          tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorC,          MOT_RIGHT,          tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="comment">//*!!Code automatically generated by &#39;ROBOTC&#39; configuration wizard               !!*//</span>

<span class="preprocessor">#include &quot;<a class="code" href="common_8h.html" title="Commonly used functions used by drivers.">drivers/common.h</a>&quot;</span>
<span class="preprocessor">#include &quot;<a class="code" href="benedettelli-nxt2wifi_8h.html" title="Dani&amp;#39;s WiFi Sensor.">drivers/benedettelli-nxt2wifi.h</a>&quot;</span>

<span class="preprocessor">#define DRIVESPEED  50</span>
<span class="preprocessor"></span><span class="preprocessor">#define TURNSPEED   20</span>
<span class="preprocessor"></span><span class="preprocessor">#define SHOOTSPEED  30</span>
<span class="preprocessor"></span>
<span class="keywordtype">string</span> IPaddress = <span class="stringliteral">&quot;0.0.0.0&quot;</span>;
<span class="keywordtype">string</span> connStatus = <span class="stringliteral">&quot;disconnected&quot;</span>;
<span class="keywordtype">string</span> dataStrings[5];
<a name="a0"></a><a class="code" href="group__common.html#gacc2928041c5ff8e7f2ddef1e88431e80">tHugeByteArray</a> data;

<span class="keywordtype">int</span> prevDetectedColour = 0;
<span class="keywordtype">int</span> currDetectedColour = 0;

<span class="keywordtype">int</span> prevBatteryLevel = 0;
<span class="keywordtype">int</span> currBatteryLevel = 0;

<span class="keywordtype">int</span> prevTouchState = 0;
<span class="keywordtype">int</span> currTouchState = 0;

<span class="keywordtype">int</span> prevSonarDistance = 0;
<span class="keywordtype">int</span> currSonarDistance = 0;

<span class="keywordtype">long</span> prevEncMotorA = 0;
<span class="keywordtype">long</span> currEncMotorA = 0;
<span class="keywordtype">long</span> prevEncMotorB = 0;
<span class="keywordtype">long</span> currEncMotorB = 0;
<span class="keywordtype">long</span> prevEncMotorC = 0;
<span class="keywordtype">long</span> currEncMotorC = 0;


task updateScreen()
{
        <span class="keywordflow">while</span>(<span class="keyword">true</span>)
        {
                nxtDisplayTextLine(0, <span class="stringliteral">&quot;Stat: %s&quot;</span>, connStatus);
                nxtDisplayTextLine(1, <span class="stringliteral">&quot;%s&quot;</span>,IPaddress);
                nxtDisplayTextLine(2, <span class="stringliteral">&quot;-------------------&quot;</span>);
                nxtDisplayTextLine(3, <span class="stringliteral">&quot;%s&quot;</span>, dataStrings[0]);
                nxtDisplayTextLine(4, <span class="stringliteral">&quot;%s&quot;</span>, dataStrings[1]);
                nxtDisplayTextLine(5, <span class="stringliteral">&quot;%s&quot;</span>, dataStrings[2]);
                nxtDisplayTextLine(6, <span class="stringliteral">&quot;%s&quot;</span>, dataStrings[3]);
                nxtDisplayTextLine(7, <span class="stringliteral">&quot;%s&quot;</span>, dataStrings[4]);
                wait1Msec(100);
        }
}

<span class="keywordtype">void</span> handleColour(ubyte state)
{
        <span class="keywordflow">switch</span> (state)
        {
        <span class="keywordflow">case</span> 3: SensorType[COLOUR] = sensorCOLORFULL;
                <span class="keywordflow">break</span>;
        <span class="keywordflow">case</span> 4: SensorType[COLOUR] = sensorCOLORRED;
                <span class="keywordflow">break</span>;
        <span class="keywordflow">case</span> 5: SensorType[COLOUR] = sensorCOLORGREEN;
                <span class="keywordflow">break</span>;
        <span class="keywordflow">case</span> 6: SensorType[COLOUR] = sensorCOLORBLUE;
                <span class="keywordflow">break</span>;
        <span class="keywordflow">case</span> 7: SensorType[COLOUR] = sensorCOLORNONE;
                <span class="keywordflow">break</span>;
        }
}

<span class="keywordtype">void</span> doStraight(ubyte state)
{
        motor[MOT_LEFT]  = state * DRIVESPEED;
        motor[MOT_RIGHT] = state * DRIVESPEED;
}

<span class="keywordtype">void</span> doRight(ubyte state)
{
        motor[MOT_LEFT]  = state *   TURNSPEED;
        motor[MOT_RIGHT] = state * -TURNSPEED;
}

<span class="keywordtype">void</span> doStop(ubyte state)
{
        motor[MOT_LEFT]  = 0;
        motor[MOT_RIGHT] = 0;
}

<span class="keywordtype">void</span> doLeft(ubyte state)
{
        motor[MOT_LEFT]  = state * -TURNSPEED;
        motor[MOT_RIGHT] = state *  TURNSPEED;
}

<span class="keywordtype">void</span> doReverse(ubyte state)
{
        motor[MOT_LEFT]  = state * -DRIVESPEED;
        motor[MOT_RIGHT] = state * -DRIVESPEED;
}

<span class="keywordtype">void</span> doAction(ubyte state)
{
        motor[MOT_ACTION] = state * SHOOTSPEED;
}


task main ()
{
        ubyte type;
        ubyte ID;
        ubyte state;
        ubyte value;
        <span class="keywordtype">string</span> dataString;
        <span class="keywordtype">string</span> tmpString;

        <span class="comment">// initialise the port, etc</span>
        <a name="a1"></a><a class="code" href="group__common-rs485.html#ga681eadb60516210bbd261bc50d80dab7">RS485initLib</a>();

        StartTask(updateScreen);

        <span class="comment">// Disconnect if already connected</span>
        <a name="a2"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga8ec5d0f6add0e5d953ad690f109a3253">N2WDisconnect</a>();
        <a name="a3"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
        wait1Msec(1000);
  <span class="keywordflow">if</span> (!<a name="a4"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga518cfe843bc8f4d042405227a0a2f4ff">N2WCustomExist</a>())
  {
    StopTask(updateScreen);
    wait1Msec(50);
    eraseDisplay();
    PlaySound(soundException);
    nxtDisplayCenteredBigTextLine(1, <span class="stringliteral">&quot;ERROR&quot;</span>);
    nxtDisplayTextLine(3, <span class="stringliteral">&quot;No custom profile&quot;</span>);
    nxtDisplayTextLine(4, <span class="stringliteral">&quot;configured!!&quot;</span>);
    <span class="keywordflow">while</span>(<span class="keyword">true</span>) EndTimeSlice();
  }

  <a name="a5"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#gac54a69b33b3a29473e8b4e80f993a401">N2WLoad</a>();

        wait1Msec(100);

        <a name="a6"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#gaa0c803e53ea5cd1221aed19eea5463f4">N2WConnect</a>(<span class="keyword">true</span>);
        connStatus = <span class="stringliteral">&quot;connecting&quot;</span>;

        <span class="keywordflow">while</span> (!<a name="a7"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga2012c5479768124f2083dc643d7b4573">N2WConnected</a>()) wait1Msec(100);
        wait1Msec(1000);

        connStatus = <span class="stringliteral">&quot;connected&quot;</span>;
        PlaySound(soundBeepBeep);

        wait1Msec(3000);
        <a name="a8"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga75f932c01ab7aaf3ba965179b2773215">N2WgetIP</a>(IPaddress);

        wait1Msec(1000);
        <span class="comment">//                123456789012345</span>
        dataStrings[0] = <span class="stringliteral">&quot;Tch | Snr | Clr&quot;</span>;
        <span class="comment">//                on  | 011 |   1&quot;</span>
        <span class="keywordflow">while</span> (<span class="keyword">true</span>)
        {
                <span class="keywordflow">if</span> (<a name="a9"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga51eb3a733a2308f77d380db47496397e">N2WreadWS</a>(type, ID, state, value))
                {
                        writeDebugStreamLine(<span class="stringliteral">&quot;btn: %d, state: %d&quot;</span>, ID, state);
                        <span class="keywordflow">switch</span> (ID)
                        {
                        <span class="keywordflow">case</span> 1: doStraight(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 3: doRight(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 4: doStop(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 5: doLeft(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 7: doReverse(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 9: doAction(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">case</span> 11: handleColour(state); <span class="keywordflow">break</span>;
                        <span class="keywordflow">default</span>: <span class="keywordflow">break</span>;
                        }
                }

                <span class="comment">// All values are only updated when they&#39;ve changed.</span>
                <span class="comment">// This cuts backs drastically on the number of messages</span>
                <span class="comment">// that have to be sent to the NXT2WIFI</span>

                <span class="comment">// Fetch the state of the Touch Sensor</span>
                <span class="comment">// This value is displayed by field 0 (in0) on the page</span>
                currTouchState = SensorBoolean[TOUCH];
                <span class="keywordflow">if</span> (currTouchState != prevTouchState)
                {
                  memset(data, 0, <span class="keyword">sizeof</span>(data));
                        data[0] = (currTouchState) ? <span class="charliteral">&#39;1&#39;</span> : <span class="charliteral">&#39;0&#39;</span>;
                        <a name="a10"></a><a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 0, data, 2);
                        prevTouchState = currTouchState;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the currently detected colour.</span>
                <span class="comment">// This value is displayed by field 1 (in1) on the page</span>
                currDetectedColour = SensorValue[COLOUR];
                <span class="keywordflow">if</span> (currDetectedColour != prevDetectedColour)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currDetectedColour);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 1, data, strlen(dataString));
                        prevDetectedColour = currDetectedColour;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the distance detected by the sonar sensor</span>
                <span class="comment">// This value is displayed by field 2 (in2) on the page</span>
                currSonarDistance = SensorValue[SONAR];
                <span class="keywordflow">if</span> (currSonarDistance != prevSonarDistance)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currSonarDistance);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 2, data, strlen(dataString));
                        prevSonarDistance = currSonarDistance;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the tacho count for motor A</span>
                <span class="comment">// This value is displayed by field 3 (in3) on the page</span>
                currEncMotorA = nMotorEncoder[MOT_ACTION];
                <span class="keywordflow">if</span> (currEncMotorA != prevEncMotorA)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currEncMotorA);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 3, data, strlen(dataString));
                        prevEncMotorA = currEncMotorA;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the tacho count for motor B</span>
                <span class="comment">// This value is displayed by field 4 (in4) on the page</span>
                <span class="comment">//currEncMotorB = nMotorEncoder[MOT_LEFT];</span>
                <span class="keywordflow">if</span> (currEncMotorB != prevEncMotorB)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currEncMotorB);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 4, data, strlen(dataString));
                        prevEncMotorB = currEncMotorB;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the tacho count for motor C</span>
                <span class="comment">// This value is displayed by field 5 (in5) on the page</span>
                currEncMotorC = nMotorEncoder[MOT_RIGHT];
                <span class="keywordflow">if</span> (currEncMotorC != prevEncMotorC)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currEncMotorC);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 5, data, strlen(dataString));
                        prevEncMotorC = currEncMotorC;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                <span class="comment">// Fetch the current voltage level.  The average one</span>
                <span class="comment">// works best, the other one jumps around too much.</span>
                <span class="comment">// This value is displayed by field 6 (in6) on the page</span>

                currBatteryLevel = nAvgBatteryLevel;
                <span class="keywordflow">if</span> (currBatteryLevel != prevBatteryLevel)
                {
                        sprintf(dataString, <span class="stringliteral">&quot;%d&quot;</span>, currBatteryLevel);
                        memcpy(data, dataString, strlen(dataString));
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga007dff56d153169bea6f5419d05c61b9">N2WwriteWS</a>(1, 6, data, strlen(dataString));
                        prevBatteryLevel = currBatteryLevel;
                        <a class="code" href="group___n_x_t2_w_i_f_i.html#ga8cad34ee07c9a294819f0070c94e1720">N2WchillOut</a>();
                }

                sprintf(dataStrings[2], <span class="stringliteral">&quot;A: %d&quot;</span>, currEncMotorA);
                sprintf(dataStrings[3], <span class="stringliteral">&quot;B: %d&quot;</span>, currEncMotorB);
                sprintf(dataStrings[4], <span class="stringliteral">&quot;C: %d&quot;</span>, currEncMotorC);
          sprintf(tmpString, <span class="stringliteral">&quot;%s | %3d&quot;</span>, (currTouchState == 0) ? <span class="stringliteral">&quot;off&quot;</span> : <span class="stringliteral">&quot;on &quot;</span>, currSonarDistance);
                sprintf(dataStrings[1], <span class="stringliteral">&quot;%s | %3d&quot;</span>, tmpString, currDetectedColour);
        }
}
</pre></div> </div>
</div>
<hr class="footer"/><address class="footer"><small>Generated on Wed Mar 13 2013 19:56:41 for ROBOTC Drivers by&#160;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.2 </small></address>
</body>
</html>
